#include <windows.h>


#include <opencv2\opencv.hpp>
#include <stdio.h>

#include <vector>
#include "Console.h"
#include "Util.h"
#include "ImageMatch.h"
#include "..\MathAux\Matrix\matrix.h"

using namespace std;

CConsole g_Console;
#define EPSILON 10e-10






using namespace cv;
Point2d	operator* (const Point2d& rhs, const Point2d& lhs)
{
	return Point2d(rhs.x * lhs.x, rhs.y * lhs.y);

}

double* GetMatrixDataAsDouble(cv::Mat A)
{
	VISIO_ASSERT(A.isContinuous());
	return (double*)A.data;
}

namespace Utils
{

cv::Mat ExpandToHVector(cv::Mat vec)
{
	if (vec.rows == 4)
	{
		return vec;
	}
	else
	{
		VISIO_ASSERT(vec.rows == 3);
		cv::Mat hVec(4, 1, DataType<double>::type);
		cv::Mat subVec = hVec(Range(0,3), Range(0,1));
		vec.copyTo(subVec);
		hVec.at<double>(3,0) = 1.0;
		return hVec;
	}
}
Point2d MoveFromScreenSpace(const Point2d& p, int width, int height)
{
// 	int HalfHeight = height / 2;
// 	int HalfWidth = width / 2;
// 	Point2f newP = p - Point2f(HalfWidth, HalfHeight);
// 	newP = newP * Point2f(1.0f / (float)HalfWidth, -1.0f / (float)HalfWidth);
	Point2d newP = p;
	return newP;
}

Matrix4f ConvertCVtoM3D( cv::Mat cvMat)
{
	VISIO_ASSERT((cvMat.rows <= 4) && (cvMat.cols <= 4));
	Matrix4f new3DMat;
	for (int row = 0; row < cvMat.rows; row++)
	{
		for (int col = 0; col < cvMat.cols; col++)
		{
			double val = cvMat.at<double>(row, col);
			new3DMat.m[row][col] = val;
		}
	}
	return new3DMat;
}

Mat VecToMat(const Vec3d& v)
{
	Mat M(1,3, DataType<double>::type);
	for (UINT i = 0; i < 3; i++)
	{
		M.at<double>(0,i) = v[i];
	}
	return M;
}

//creating the cross matrix
void CreateCrossMatrix(Vec<double, 3> vec, Mat& CrossMat)
{
	Mat row[3];
	row[0] = (Mat_<double>(1,3) << 0, -vec[2], vec[1]);
	row[1] = (Mat_<double>(1,3) << vec[2], 0, -vec[0]);
	row[2] = (Mat_<double>(1,3) << -vec[1], vec[0], 0);
	for (UINT i = 0; i < 3; i++)
	{
		row[i].copyTo(CrossMat.row(i));
	}
	
}

void findLeftNullSpace(Mat& M, Mat& NullBase)
{
	Mat R,Q;
	RQDecomp3x3(M, R, Q);
	UINT zeroLineIndex = -1;
	PrintMat<double>(R);
	PrintMat<double>(Q);
	// first we find the row with the zero on the diagonal
	for (int i = 0; i < M.cols; i++)
	{
		double diagElement = R.at<double>(i,i);
		if (abs(diagElement) < EPSILON)
		{
			zeroLineIndex = i; 
			break;
		}
	}

	// make sure we find the zero element in the diag
	VISIO_ASSERT(zeroLineIndex != -1);

	Mat vec1 = M.col((zeroLineIndex + 1) % 3);
	Mat vec2 = M.col((zeroLineIndex + 2) % 3);

	//since we are looking for a vector that is perpenciullar to the other rows
	NullBase = vec1.t().cross(vec2.t());
}

cv::Mat Project3D(cv::Mat src)
{
	cv::Mat ret = (cv::Mat_<double>(2,1) << CV_MAT(src, 0, 0) /  CV_MAT(src, 2, 0), CV_MAT(src, 1, 0) /  CV_MAT(src, 2, 0));
	return ret;
}

Point2d Project3D(Point3d p)
{
	Point2d ret(p.x / p.z, p.y / p.z);
	return ret;
}

Utils::SSVD ComputeSVD( cv::Mat A, bool bUseBundleMath )
{
	//currenlty only works for 3x3 matrix
	VISIO_ASSERT(A.cols == 3 && A.rows == 3);
	Utils::SSVD retSVD;
	if (!bUseBundleMath)
	{
		cv::SVD svd(A);
		retSVD.u = svd.u;
		retSVD.w = svd.w;
		retSVD.vt = svd.vt;
		
	}
	else
	{
		 double U[9], S[3], VT[9];
		 VISIO_ASSERT(A.isContinuous());
		 dgesvd_driver(3, 3, (double*)A.data, U, S, VT);
		 retSVD.u = INIT_MAT_ARRAY(3,3,U);
		 retSVD.w = INIT_MAT_ARRAY(3,1,S);
		 retSVD.vt = INIT_MAT_ARRAY(3,3,VT);
	}
	return retSVD;
}

cv::Mat Diag(int size, double* vals)
{
	cv::Mat D = EYE(size);
	for (int i = 0; i < size; i++)
	{
		CV_MAT(D, i, i) = vals[i];
	}

	return D;
}
double Dot(Point2d a, Point2d b)
{
	return (a.x * b.x + a.y * b.y);
}
cv::Point3d Point3dFromArray( double* p )
{
	return Point3d(p[0], p[1], p[2]);
}

cv::Point2d Point2dFromArray( double* p)
{
	return Point2d(p[0], p[1]);
}

cv::Mat ConvertCamCenterToTranslation( cv::Mat R, cv::Mat c )
{
	return (R*c) * -1.0;
}

double Dot( cv::Mat a, cv::Mat b )
{
	Mat a_b = a*b;
	double* ret = GetMatrixDataAsDouble(a_b);
	return *ret;
}

void InvertDistortion(int n_in, int n_out, double r0, double r1, 
	double *k_in, double *k_out)
{
	const int NUM_SAMPLES = 20;

	int num_eqns = NUM_SAMPLES;
	int num_vars = n_out;

	double *A = new double[num_eqns * num_vars];
	double *b = new double[num_eqns];

	for (int i = 0; i < NUM_SAMPLES; i++) {
		double t = r0 + i * (r1 - r0) / (NUM_SAMPLES - 1);

		double p = 1.0;
		double a = 0.0;
		for (int j = 0; j < n_in; j++) {
			a += p * k_in[j];
			p = p * t;
		}

		double ap = 1.0;
		for (int j = 0; j < n_out; j++) {
			A[i * num_vars + j] = ap;
			ap = ap * a;
		}

		b[i] = t;
	}

	dgelsy_driver(A, b, k_out, num_eqns, num_vars, 1);

	delete [] A;
	delete [] b;
}

}